#include "robot.hpp"
#include <iostream>

#include "crc16.hpp"

Robot::Robot(const char* port, int baudrate):serial(port,baudrate)
{
	
}

Robot::~Robot()
{
	
}

int  Robot::connect()
{
	return serial.connect();
}

void Robot::release()
{
	serial.release();
}

void Robot::cmd(uint8_t fb, uint8_t lr, uint8_t rotate, uint8_t pan, uint8_t tilt, uint8_t fire)
{
	cbuf[0] = ROBOT_CMD_HEADER;
	cbuf[1] = fb;
	cbuf[2] = lr;
	cbuf[3] = rotate;
	cbuf[4] = pan;
	cbuf[5] = tilt;
	cbuf[6] = fire;
	CRC16Append(cbuf, ROBOT_CMD_BUF_LEN);
	serial.tx(cbuf, ROBOT_CMD_BUF_LEN);
}
